#include "rads_business.hpp"
std::mutex g_db_mutex;
void radarThreadFunction(std::vector<car> pathPoints, int loop_count = 1) {
    // PID控制器
    EnhancedAnglePIDController enhanced_pid(CONFIG_KP, CONFIG_KI, CONFIG_KD, CONFIG_MIN_OUTPUT, CONFIG_MAX_OUTPUT, stop_car, Stop_lidar);
    double distance;
    double NDT_JDC;
    double NDT_JDC_new;
    int speed = MINSPEED;       // 初始速度
    int maxspeed = MAXSPEED; // 最大速度
    int minspeed = MINSPEED;
    int speed_new;
    int api_change_counter_update = 0;
    int speed_change_counter_update = 0;
    int current_loop = 0; // 当前循环次数

    // 新增：绕道模式控制变量
    bool in_bypass_mode = false;       // 是否处于绕道模式
    int bypass_start_index = -1;       // 绕道开始的点位索引
    const int BYPASS_POINT_COUNT = 4;  // 绕道点数量（与偏移点数量一致）

    // 创建路径点的副本用于偏移操作
    std::vector<car> adjustedPathPoints = pathPoints;

    while (current_loop < loop_count && !stopRadarThread.load()) {
        // 每次循环重置绕道状态
        in_bypass_mode = false;
        bypass_start_index = -1;
        
        int snapped_index = findSnappedWaypoint(adjustedPathPoints); // 吸附到调整后的路径点
        if (snapped_index != -1) {
            const car &target_pointxf = adjustedPathPoints[snapped_index];
            if (lidar_rtk.matching_rate > 45.0) {
                std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.x << ", " << target_pointxf.y << ")" << std::endl;
            } else {
                std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.rtk_x << ", " << target_pointxf.rtk_y << ")" << std::endl;
            }
        } else {
            std::cout << "未找到有效吸附点" << std::endl;
        }

        // 重置调整后的路径点（每次循环开始时使用原始路径点）
        adjustedPathPoints = pathPoints;

        while (snapped_index < adjustedPathPoints.size() && !stopRadarThread.load() && (lidar_rtk.matching_rate > 45 || lidar_rtk.posi_flag >= 4)) {
            // 优先处理Stop_lidar == 1的停车逻辑
            if (Stop_lidar == 1) {
                pubdrive(0, 2, 18000, 0); // 执行停车
                std::cout << "检测到Stop_lidar == 1，执行紧急停车" << std::endl;
                speed = MINSPEED;
                usleep(20000);
                continue;
            }

            api_change_counter_update++;
            speed_change_counter_update++;
            
            // 获取当前车辆位置和角度
            double current_x = lidar_rtk.matching_rate > 45.0 ? lidar_rtk.x : lidar_rtk.rtk_x;
            double current_y = lidar_rtk.matching_rate > 45.0 ? lidar_rtk.y : lidar_rtk.rtk_y;
            double newld_heading =lidar_rtk.ld_heading - 180.0;
             if (newld_heading < 0){
                     newld_heading =newld_heading + 360.0; 
                }
            if(newld_heading > 360){
                newld_heading =newld_heading - 360.0;
            }
            std::cout << "newld_heading:::" << newld_heading << std::endl;
            double current_angle = lidar_rtk.matching_rate > 45.0 ? newld_heading * M_PI / 180.0 : lidar_rtk.yaw * M_PI / 180.0; // 转换为弧度
             std::cout << "current_angle:::" << current_angle << std::endl;
            // 1. 获取当前目标点（使用调整后的路径点）
            car &current_target = adjustedPathPoints[snapped_index];
            // 计算车到目标点的距离
            if (lidar_rtk.matching_rate > 45.0) {
                distance = distance_count(lidar_rtk.x, lidar_rtk.y, current_target.x, current_target.y);
            } else {
                distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, current_target.rtk_x, current_target.rtk_y);
            }

            if (distance < DRIVE_DISRANCE) {
                std::cout << "已到达点位 [" << current_target.num_point << "]" << std::endl;
                snapped_index++; // 移向下一个点
                
                // 检查是否已完成绕道（到达绕道开始点后的第4个点）
                if (in_bypass_mode && snapped_index >= bypass_start_index + BYPASS_POINT_COUNT) {
                    in_bypass_mode = false;
                    bypass_start_index = -1;
                    std::cout << "已完成绕道，退出绕道模式" << std::endl;
                    // 重置路径点为原始路径，准备重新判断
                    adjustedPathPoints = pathPoints;
                }
                continue;
            }

            // 障碍处理与点位偏移（仅在未进入绕道模式时执行）
            if (!in_bypass_mode && stop_car == 1 && Stop_lidar_qian == 1) {  
                // 计算需要偏移的点数量（最多4个点）
                int points_to_offset = std::min(BYPASS_POINT_COUNT, (int)adjustedPathPoints.size() - snapped_index);
                
                // 输出调试信息：当前角度（弧度和度）
                double current_angle_deg = current_angle * 180.0 / M_PI;
                std::cout << "当前角度: " << current_angle << " 弧度 (" << current_angle_deg << " 度)" << std::endl;
                
                if(Stop_lidar_l != 1){   // 左侧无障碍，向左偏移
                    std::cout << "前方有障碍，进入向左绕道模式，处理" << points_to_offset << "个点" << std::endl;
                    
                    // 标记进入绕道模式
                    in_bypass_mode = true;
                    bypass_start_index = snapped_index;
                    
                    // 计算偏移量（向左偏移2米）
                    // 修正：基于车头方向重新定义左右偏移的三角函数关系
                    double offset_distance = 1.0;
                    // 向左偏移：垂直车头方向向左，修正角度计算
                    double delta_x = -offset_distance * cos(current_angle);  // X方向偏移（主要分量）
                    double delta_y = offset_distance * sin(current_angle);   // Y方向偏移（次要分量）
                    
                    // 输出偏移量调试信息
                    std::cout << "向左偏移量: delta_x = " << delta_x << ", delta_y = " << delta_y << std::endl;
                    
                    // 对前方点进行偏移处理
                    for(int i = 0; i < points_to_offset; i++){
                        int index = snapped_index + i;
                        if(index >= adjustedPathPoints.size()) break;
                        
                        // 保存原始坐标用于调试
                        double original_x = adjustedPathPoints[index].x;
                        double original_y = adjustedPathPoints[index].y;
                        
                        // 应用偏移
                        adjustedPathPoints[index].x += delta_x;
                        adjustedPathPoints[index].y += delta_y;
                        adjustedPathPoints[index].rtk_x += delta_x;
                        adjustedPathPoints[index].rtk_y += delta_y;
                        
                        // 输出偏移前后坐标对比
                        std::cout << "点[" << index << "] 偏移前: (" << original_x << ", " << original_y 
                                  << ") 偏移后: (" << adjustedPathPoints[index].x << ", " << adjustedPathPoints[index].y << ")" << std::endl;
                    }
                    // 更新当前目标点为偏移后的值
                    current_target = adjustedPathPoints[snapped_index];
                }
                else if(Stop_lidar_r != 1){   // 右侧无障碍，向右偏移
                    std::cout << "前方有障碍，进入向右绕道模式，处理" << points_to_offset << "个点" << std::endl;
                    
                    // 标记进入绕道模式
                    in_bypass_mode = true;
                    bypass_start_index = snapped_index;
                    
                    // 计算偏移量（向右偏移2米）
                    // 修正：基于车头方向重新定义左右偏移的三角函数关系
                    double offset_distance = 1.0;
                    // 向右偏移：垂直车头方向向右，修正角度计算
                    double delta_x = offset_distance * cos(current_angle);   // X方向偏移（主要分量）
                    double delta_y = -offset_distance * sin(current_angle);  // Y方向偏移（次要分量）
                    
                    // 输出偏移量调试信息
                    std::cout << "向右偏移量: delta_x = " << delta_x << ", delta_y = " << delta_y << std::endl;
                    
                    // 对前方点进行偏移处理
                    for(int i = 0; i < points_to_offset; i++){
                        int index = snapped_index + i;
                        if(index >= adjustedPathPoints.size()) break;
                        
                        // 保存原始坐标用于调试
                        double original_x = adjustedPathPoints[index].x;
                        double original_y = adjustedPathPoints[index].y;
                        
                        // 应用偏移
                        adjustedPathPoints[index].x += delta_x;
                        adjustedPathPoints[index].y += delta_y;
                        adjustedPathPoints[index].rtk_x += delta_x;
                        adjustedPathPoints[index].rtk_y += delta_y;
                        
                        // 输出偏移前后坐标对比
                        std::cout << "点[" << index << "] 偏移前: (" << original_x << ", " << original_y 
                                  << ") 偏移后: (" << adjustedPathPoints[index].x << ", " << adjustedPathPoints[index].y << ")" << std::endl;
                    }
                    // 更新当前目标点为偏移后的值
                    current_target = adjustedPathPoints[snapped_index];
                }
                else {  // 两侧均有障碍，停车
                    pubdrive(0, 2, 18000, 0);
                    std::cout << "前方及两侧均有障碍，无法避让，已停车" << std::endl;
                    speed = MINSPEED;
                    usleep(20000);
                    continue; // 跳过后续控制逻辑
                }
            }
    

            // 发布进度（使用调整后的路径点）
            if (api_change_counter_update >= API_CHANGE_INTERVAL) {
                api_change_counter_update = 0;
                publishSradsackProgress(snapped_index, adjustedPathPoints, speed_new, current_target.line_id, current_target.map_id);
            }

            // 2. 计算NDT_JDC（使用偏移后的目标点）
            if (PID_STATUS == 0) {
                if (lidar_rtk.matching_rate > 45.0) {
                    NDT_JDC = calculate_NDT_JDC(current_target);
                    ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
                                    << "LIDAR Mode | 偏移后角度计算: " << NDT_JDC);
                } else {
                    NDT_JDC = rtkcalculate_NDT_JDC(current_target);
                    ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
                                    << "RTK Mode | 偏移后角度计算: " << NDT_JDC);
                }
            } else {
                if (lidar_rtk.matching_rate > 45.0) {
                    double raw_angle = calculate_NDT_JDC(current_target);
                    NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
                    ROS_INFO_STREAM("LIDAR Mode | 偏移后原始角度: " << raw_angle << " | PID输出: " << NDT_JDC);
                } else {
                    double raw_angle = rtkcalculate_NDT_JDC(current_target);
                    NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
                    ROS_INFO_STREAM("RTK Mode | 偏移后原始角度: " << raw_angle << " | PID输出: " << NDT_JDC);
                }
            }

            // 3. 速度控制逻辑
            if (ladar_1 < LFRFLRRR || ladar_2 < LFRFLRRR || ladar_3 < LFRFLRRR || ladar_4 < LFRFLRRR) {
                std::cout << "收到减速信号" << std::endl;
                if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL) {
                    speed_change_counter_update = 0;
                    speed = std::max(minspeed, speed - 1);
                }
            } else {
                if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL) {
                    speed_change_counter_update = 0;
                    speed = std::min(maxspeed, speed + 1);
                }
            }

            // 4. 角度调整
            if (PID_STATUS == 0) {
                if (std::abs(NDT_JDC) < 30 && std::abs(NDT_JDC) > 0) {
                    NDT_JDC_new = NDT_JDC * 1;
                } else if (std::abs(NDT_JDC) < 40 && std::abs(NDT_JDC) >= 30) {
                    NDT_JDC_new = NDT_JDC * 0.8;
                } else if (std::abs(NDT_JDC) > 60) {
                    NDT_JDC_new = NDT_JDC * 0.3;
                } else {
                    NDT_JDC_new = NDT_JDC;
                }
            } else {
                NDT_JDC_new = NDT_JDC;
            }

            // 5. 速度调整
            speed_new = cos(NDT_JDC_new / 114.6) * speed;
            if (std::abs(NDT_JDC) > 60 || std::abs(NDT_JDC) > 90) {
                speed_new = 0;
            }

            // 输出导航信息
            std::cout << std::fixed << std::setprecision(6) 
                      << "导航至" << (in_bypass_mode ? "绕道" : "") << "点位 [" << current_target.num_point << "]: "
                      << "(雷达:[" << current_target.x << ", " << current_target.y << "], "
                      << "RTK:[" << current_target.rtk_x << ", " << current_target.rtk_y << "])"
                      << " | 距离:" << distance << "米, 计算角度：" << NDT_JDC 
                      << ", 执行角度：" << NDT_JDC_new << ", 速度：" << speed_new << std::endl;
            pubdrive(0, 2, NDT_JDC_new * 100 + 18000.0, speed_new);
            
            usleep(20000);
        }

        current_loop++;
        if (current_loop < loop_count) {
            std::cout << "准备开始第 " << current_loop + 1 << " 次循环..." << std::endl;
            usleep(1000000);
        }
    }

    // 结束处理
    rads::rads_acks msg;
    msg.ack = 3;
    int baifenbiend = 100;
    int lichengend = 0;
    int duoshaomiaoend = 0;
    int line_id = 0;
    int map_id = 0;
    std::string progressStr = std::to_string(baifenbiend) + "," + std::to_string(lichengend) + "," + std::to_string(duoshaomiaoend) + "," + std::to_string(line_id) + "," + std::to_string(map_id);
    msg.progress = progressStr;
    sradsack_pub_.publish(msg);
    std::cout << "正常--无人驾驶结束" << std::endl;
    pubdrive(100, 2, 18000, 0);
}
    



// 自动驾驶
// 自动驾驶
// void radarThreadFunction(std::vector<car> pathPoints, int loop_count = 1) {
//     // PID
//     EnhancedAnglePIDController enhanced_pid(CONFIG_KP, CONFIG_KI, CONFIG_KD, CONFIG_MIN_OUTPUT, CONFIG_MAX_OUTPUT, stop_car, Stop_lidar);
//     double distance;
//     double NDT_JDC;
//     double NDT_JDC_new;
//     int speed = MINSPEED;       // 初始速度
//     int maxspeed = MAXSPEED; // 最大速度
//     int minspeed = MINSPEED;
//     int speed_new;
//     int api_change_counter_update = 0;
//     int speed_change_counter_update = 0;
//     int current_loop = 0; // 当前循环次数

//     while (current_loop < loop_count && !stopRadarThread.load()) {
//         int snapped_index = findSnappedWaypoint(pathPoints); // 吸附点
//         if (snapped_index != -1) {
//             const car &target_pointxf = pathPoints[snapped_index];
//             if (lidar_rtk.matching_rate > 45.0) {
//                 std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.x << ", " << target_pointxf.y << ")" << std::endl;
//             } else {
//                 std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.rtk_x << ", " << target_pointxf.rtk_y << ")" << std::endl;
//             }
//         } else {
//             std::cout << "未找到有效吸附点" << std::endl;
//         }

//         while (snapped_index < pathPoints.size() && !stopRadarThread.load() && (lidar_rtk.matching_rate > 45 || lidar_rtk.posi_flag >= 4)) {
//             api_change_counter_update++;
//             speed_change_counter_update++;
//             // 1. 获取当前目标点
//             const car &target_point = pathPoints[snapped_index];
//             // 计算车到目标点的距离
//             if (lidar_rtk.matching_rate > 45.0) {
//                 distance = distance_count(lidar_rtk.x, lidar_rtk.y, target_point.x, target_point.y);
//             } else {
//                 distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, target_point.rtk_x, target_point.rtk_y);
//             }

//             if (distance < DRIVE_DISRANCE) {
//                 std::cout << "已到达点位 [" << target_point.num_point << "]" << std::endl;
//                 snapped_index++; // 移向下一个点
//                 continue;
//             }

//             if (api_change_counter_update >= API_CHANGE_INTERVAL) {
//                 api_change_counter_update = 0; // 重置计数器
//                 publishSradsackProgress(snapped_index, pathPoints, speed_new, target_point.line_id, target_point.map_id);
//             }

//             if (PID_STATUS == 0) {
//                 // 正常计算角度
//                 if (lidar_rtk.matching_rate > 45.0) {
//                     NDT_JDC = calculate_NDT_JDC(target_point);
//                     ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
//                                     << "LIDAR Mode | Matching Rate: " << lidar_rtk.matching_rate
//                                     << " | Output: " << NDT_JDC);
//                 } else {
//                     NDT_JDC = rtkcalculate_NDT_JDC(target_point);
//                     ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
//                                     << "RTK Mode | Posi Flag: " << lidar_rtk.posi_flag
//                                     << " | Output: " << NDT_JDC);
//                 }
//             } else {
//                 // 计算角度使用 PID
//                 if (lidar_rtk.matching_rate > 45.0) {
//                     double raw_angle = calculate_NDT_JDC(target_point);
//                     NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
//                     ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
//                                     << "LIDAR Mode | Matching Rate: " << lidar_rtk.matching_rate
//                                     << " | Raw Angle: " << raw_angle
//                                     << " | PID Output: " << NDT_JDC);
//                 } else {
//                     double raw_angle = rtkcalculate_NDT_JDC(target_point);
//                     NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
//                     ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
//                                     << "RTK Mode | Posi Flag: " << lidar_rtk.posi_flag
//                                     << " | Raw Angle: " << raw_angle
//                                     << " | PID Output: " << NDT_JDC);
//                 }
//             }

//             if (stop_car == 1) { // 正常驾驶
//                 while (Stop_lidar == 1 && !stopRadarThread.load()) {
//                     pubdrive(0, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
//                     std::cout << "前方有障碍，正在停车，当前速度: " << 0 << " KM/h" << std::endl;
//                     speed = MINSPEED; // 初始速度
//                     usleep(20000);
//                 }
//                 if(Stop_lidar_qian == 1){  //前方右障碍
//                      if(Stop_lidar_l != 1){   // 车辆左边没有障碍，可以绕
//                             // 把前方的4个点（4米，1米一个点） 向左偏移 1米，
//                      }

//                      if(Stop_lidar_r != 1){   // 车辆右边没有障碍，可以绕
//                             // 把前方的4个点（4米，1米一个点） 向右偏移 1米，
//                      }

//                      if(Stop_lidar_l == 1 && Stop_lidar_r == 1){  //车辆左右都有障碍，车辆停
//                             pubdrive(0, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
//                             std::cout << "前方有障碍，正在停车，当前速度: " << 0 << " KM/h" << std::endl;
//                             speed = MINSPEED; // 初始速度
//                             usleep(20000);
//                      }
                     
//                 }

//                 // 减速
//                 if (ladar_1 < LFRFLRRR || ladar_2 < LFRFLRRR || ladar_3 < LFRFLRRR || ladar_4 < LFRFLRRR) {
//                     std::cout << "车辆正在无人驾驶,收到减速信号" << std::endl;
//                     if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL) {
//                         speed_change_counter_update = 0; // 重置计数器
//                         if (speed > minspeed) {
//                             speed = speed - 1;
//                         }
//                     }
//                 } else {
//                     // 加速--》直到最大速度
//                     if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL) {
//                         speed_change_counter_update = 0; // 重置计数器
//                         if (speed < maxspeed) {
//                             speed = speed + 1;
//                         }
//                     }
//                 }

//                 if (PID_STATUS == 0) {
//                     if (std::abs(NDT_JDC) < 30 && std::abs(NDT_JDC) > 0) {
//                         NDT_JDC_new = NDT_JDC * 1;
//                     } else if (std::abs(NDT_JDC) < 40 && std::abs(NDT_JDC) >= 30) {
//                         NDT_JDC_new = NDT_JDC * 0.8;
//                     } else if (std::abs(NDT_JDC) > 60) {
//                         NDT_JDC_new = NDT_JDC * 0.3;
//                     } else {
//                         NDT_JDC_new = NDT_JDC;
//                     }
//                 } else {
//                     NDT_JDC_new = NDT_JDC;
//                 }

//                 speed_new = cos(NDT_JDC_new / 114.6) * speed;
//                 if (std::abs(NDT_JDC) > 60) {
//                     speed_new = 0;
//                 }
//                 if (std::abs(NDT_JDC) > 90) {
//                     speed_new = 0;
//                 }

//                 // 输出导航信息current_loop < loop_count
//                 std::cout << std::fixed << std::setprecision(6) <<"循环次数："<< loop_count <<",当前第"<< current_loop +1 << "次,总共：" << pathPoints.size() << ",导航至点位 [" << target_point.num_point << "]: (雷达:["
//                           << target_point.x << ", " << target_point.y << "],rkt:[" << target_point.rtk_x << ", " << target_point.rtk_y << "])"
//                           << " | 距离:" << distance << "米,计算角度：" << NDT_JDC << ",执行角度：" << NDT_JDC_new << "速度：" << speed_new << std::endl;
//                 pubdrive(0, 2, NDT_JDC_new * 100 + 18000.0, speed_new);
//             } else {
//                 pubdrive(0, 2, 18000, 0);
//             }
//             usleep(20000);
//         }

//         current_loop++; // 增加循环计数
//         if (current_loop < loop_count) {
//             std::cout << "准备开始第 " << current_loop + 1 << " 次循环..." << std::endl;
//             usleep(1000000); // 等待1秒再开始下一次循环
//         }
//     }

//     rads::rads_acks msg;
//     msg.ack = 3;
//     int baifenbiend = 100;
//     int lichengend = 0;
//     int duoshaomiaoend = 0;
//     int line_id = 0;
//     int map_id = 0;
//     std::string progressStr = std::to_string(baifenbiend) + "," + std::to_string(lichengend) + "," + std::to_string(duoshaomiaoend) + "," + std::to_string(line_id) + "," + std::to_string(map_id);
//     msg.progress = progressStr;
//     sradsack_pub_.publish(msg);
//     std::cout << "正常--无人驾驶结束" << std::endl;
//     pubdrive(100, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
// }

// 返航驾驶
void thread_Create_line_goback_play()
{
    double distance;
    double NDT_JDC;
    double NDT_JDC_new;
    int speed = MINSPEED;       // 初始速度
    int maxspeed = MAXSPEED; // 最大速度
    int minspeed = MINSPEED;
    int speed_new;
    int speed_change_counter_update = 0;
    double current_yaw, initial_yaw, target_yaw;
    double tolerance = 10.0;
    turnaround_state = TS_READY;
    try
    {
        auto pathPoints = fetchgobackDataFromDB(); // 可能抛出异常
        if (pathPoints.empty())
        {
            std::cout << "警告：未获取到返航路线数据" << std::endl;
            return;
        }
        int snapped_index = current_target_index.load();

        while (should_run_goback_play.load())
        {
            // 边界保护
            if (snapped_index < 0)
            {
                snapped_index = 0;
            }
            else if (snapped_index >= pathPoints.size())
            {
                snapped_index = pathPoints.size() - 1;
            }

            // 处理掉头状态
            if (need_turnaround)
            {
                switch (turnaround_state)
                {
                case TS_READY:
                    // 准备掉头，调整索引到正确的下一个点
                    if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
                    {
                        initial_yaw = lidar_rtk.heading;
                    }
                    else
                    {
                        initial_yaw = lidar_gobackmsg.ld_heading;
                    }

                    target_yaw = initial_yaw + 180.0;
                    if (snapped_index == 0)
                    {
                        // 起点掉头，只能向前进
                        if (pathPoints.size() > 1)
                        {
                            snapped_index = 1;
                            turnaround_state = TS_TURNING;
                            std::cout << "开始掉头，新目标索引: " << snapped_index << std::endl;
                        }
                        else
                        {
                            std::cout << "路径长度不足，无法掉头" << std::endl;
                            need_turnaround = false;
                        }
                    }
                    else if (snapped_index == pathPoints.size() - 1)
                    {
                        // 终点掉头，只能向后退
                        if (pathPoints.size() > 1)
                        {
                            snapped_index = pathPoints.size() - 2;
                            turnaround_state = TS_TURNING;
                            std::cout << "开始掉头，新目标索引: " << snapped_index << std::endl;
                        }
                        else
                        {
                            std::cout << "路径长度不足，无法掉头" << std::endl;
                            need_turnaround = false;
                        }
                    }
                    else
                    {
                        // 中间位置掉头，根据新方向调整
                        if (goback_forward)
                        {
                            // 掉头后正向行驶，确保下一个点在前方
                            snapped_index = std::min(static_cast<int>(pathPoints.size() - 1), snapped_index + 1);
                        }
                        else
                        {
                            // 掉头后反向行驶，确保下一个点在后方
                            snapped_index = std::max(0, snapped_index - 1);
                        }
                        turnaround_state = TS_TURNING;
                        std::cout << "开始掉头，新目标索引: " << snapped_index << std::endl;
                    }
                    current_target_index = snapped_index;
                    break;

                case TS_TURNING:
                {
                    // 执行掉头操作
                    // double current_yaw, initial_yaw, target_yaw;
                    // double tolerance = 10.0;

                    // 根据定位状态选择数据源
                    if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
                    {
                        current_yaw = lidar_rtk.heading;
                    }
                    else
                    {
                        current_yaw = lidar_gobackmsg.ld_heading;
                    }

                    speed = MINSPEED;

                    std::cout << "Current yaw: " << current_yaw << ", Target yaw: " << target_yaw
                              << ", 角度差: " << fabs(fmod(current_yaw - target_yaw, 360.0)) << std::endl;

                    if (fabs(fmod(current_yaw - target_yaw, 360.0)) <= tolerance)
                    {
                        // 掉头完成
                        turnaround_state = TS_COMPLETED;
                        need_turnaround = false;
                        pubdrive(0, 2, 18000, 0);
                        std::cout << "掉头完成，驶向新目标点" << std::endl;
                    }
                    else
                    {
                        if(stop_car==1){
                            // 继续掉头
                            if (fabs(fmod(current_yaw - target_yaw, 360.0)) <= 90)
                            {
                                pubdrive(0, 2, 20 * 100 + 18000, 0);
                            }
                            else
                            {
                                pubdrive(0, 2, 40 * 100 + 18000, 0);
                            }
                        }else{
                            pubdrive(0, 2, 18000, 0);
                            std::cout << "掉头未完成，车辆暂停中" << std::endl;
                        }
                    }
                    break;
                }

                case TS_COMPLETED:
                    // 掉头已完成，重置状态
                    turnaround_state = TS_READY;
                    break;
                }

                // 如果正在掉头，跳过后续逻辑
                if (turnaround_state == TS_TURNING)
                {
                    usleep(20000);
                    continue;
                }
            }

            const go_back &target_point = pathPoints[snapped_index];
            speed_change_counter_update++;

            // 计算车到目标点的距离
            if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
            {
                distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, target_point.rtk_x, target_point.rtk_y);
            }
            else
            {
                distance = distance_count(lidar_gobackmsg.x, lidar_gobackmsg.y, target_point.x, target_point.y);
            }

            if (distance < DRIVE_DISRANCE)
            {
                std::cout << "已到达点位 [" << target_point.num_point << "]" << std::endl;

                if (goback_forward)
                {
                    if (snapped_index < pathPoints.size() - 1)
                        snapped_index++;
                }
                else
                {
                    if (snapped_index > 0)
                        snapped_index--;
                }
                current_target_index = snapped_index;
                continue;
            }

            // 计算导航角度
            if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
            {
                NDT_JDC = goback_rtkcalculate_NDT_JDC(target_point);
                std::cout << "返航-当前使用RTK-posi_flag:" << lidar_rtk.posi_flag << std::endl;
            }
            else
            {
                NDT_JDC = goback_calculate_NDT_JDC(target_point);
                std::cout << "返航-当前使用雷达" << std::endl;
            }

            // 障碍物检测与速度控制
            if (stop_car == 1)
            { // 正常驾驶
                while ((Stop_lidar == 1 || ladar_5 < FLLR || ladar_6 < FLLR) && should_run_goback_play.load())
                {
                    pubdrive(0, 2, 18000, 0);
                    std::cout << ",前方有障碍，正在停车，当前速度: " << 0 << " KM/h" << std::endl;
                    speed = MINSPEED;
                    usleep(20000);
                }

                // 减速逻辑
                if (ladar_1 < LFRFLRRR || ladar_2 < LFRFLRRR || ladar_3 < LFRFLRRR || ladar_4 < LFRFLRRR)
                {
                    std::cout << "车辆正在无人驾驶,收到减速信号" << std::endl;
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed > minspeed)
                        {
                            speed = speed - 1;
                        }
                    }
                }
                else
                {
                    // 加速--》直到最大速度
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed < maxspeed)
                        {
                            speed = speed + 1;
                        }
                    }
                }

                // 角度调整与速度计算
                if (std::abs(NDT_JDC) < 30 && std::abs(NDT_JDC) > 0)
                {
                    NDT_JDC_new = NDT_JDC * 1.5;
                }
                else if (std::abs(NDT_JDC) < 40 && std::abs(NDT_JDC) >= 30)
                {
                    NDT_JDC_new = NDT_JDC * 1.2;
                }
                else if (std::abs(NDT_JDC) > 60)
                {
                    NDT_JDC_new = NDT_JDC * 0.3;
                }
                else
                {
                    NDT_JDC_new = NDT_JDC;
                }

                speed_new = cos(NDT_JDC_new / 114.6) * speed;
                if (std::abs(NDT_JDC) > 60)
                {
                    speed_new = 0;
                }
                if (std::abs(NDT_JDC) > 90)
                {
                    speed_new = 0;
                }

                // 输出导航信息
                std::cout << std::fixed << std::setprecision(6)
                          << "总共：" << pathPoints.size()
                          << ",导航至点位 [" << target_point.num_point
                          << "],当前的索引值[" << snapped_index << "]: ("
                          << target_point.x << ", " << target_point.y
                          << ",rkt:" << target_point.rtk_x << ", " << target_point.rtk_y << ")"
                          << " | 距离:" << distance
                          << "米,计算角度：" << NDT_JDC
                          << ",执行角度：" << NDT_JDC_new
                          << "速度：" << speed_new << std::endl;

                pubdrive(0, 2, NDT_JDC_new * 100 + 18000.0, speed_new);
            }
            else
            {
                pubdrive(0, 2, 18000, 0);
            }

            usleep(20000);
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "返航线程异常: " << e.what() << std::endl;
        should_run_goback_play.store(false);
    }
    std::cout << "返航结束" << std::endl;
    should_run_goback_play.store(false);
}
// 马踏飞燕
void thread_Create_line_all(int map_id)
{
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
    while (should_run_all.load())
    {
        use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.cmd = 888;
            rads_acks_msg.map_id = map_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建基本路线,在地图号：" << map_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建基本路线,在地图号：" << map_id << ",一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 记忆采集点
void thread_Create_line_goback()
{
    // 清空表
    delete_go_back();
    // 发布话题
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_gobackmsg.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_gobackmsg.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_gobackmsg.ld_heading;
    rads::rads_acks rads_acks_msg;
    rads_acks_msg.header.stamp = ros::Time::now();
    rads_acks_msg.cmd = 999;
    rads::rads_trackext trajectory_ext;
    trajectory_ext.point_count = caidiannum_point;
    trajectory_ext.x = lidar_gobackmsg.x;
    trajectory_ext.y = lidar_gobackmsg.y;
    trajectory_ext.z = lidar_gobackmsg.z;
    trajectory_ext.rtk_x = lidar_rtk.rtk_x;
    trajectory_ext.rtk_y = lidar_rtk.rtk_y;
    trajectory_ext.rtk_z = lidar_rtk.rtk_z;
    trajectory_ext.yaw = lidar_rtk.ld_heading;
    trajectory_ext.ld_heading = lidar_rtk.ld_heading;
    trajectory_ext.longitude = lidar_rtk.longitude;
    trajectory_ext.latitude = lidar_rtk.latitude;
    trajectory_ext.heading = lidar_rtk.heading;
    trajectory_ext.posi_flag = lidar_rtk.posi_flag;
    trajectory_ext.head_flag = lidar_rtk.head_flag;
    rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
    sradsack_pub_.publish(rads_acks_msg);
    cout << "创建记忆路线,踩点成功！当前点个数: " << caidiannum_point << " | 模式:" << (use_rtk ? "RTK" : "激光")
         << ",distance:" << distance << ",heading_diff:" << heading_diff << endl;
    caidiannum_point++;
    while (should_run_goback.load())
    {
        use_rtk = (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_gobackmsg.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_gobackmsg.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_gobackmsg.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.cmd = 999;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_gobackmsg.x;
            trajectory_ext.y = lidar_gobackmsg.y;
            trajectory_ext.z = lidar_gobackmsg.z;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建记忆路线,踩点成功！当前点个数: " << caidiannum_point << " | 模式:" << (use_rtk ? "RTK" : "激光")
                 << ",distance:" << distance << ",heading_diff:" << heading_diff << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束记忆路线,一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 踩点带路线和地图
void thread_Create_line(int map_id, int path_id, string pro_nema)
{
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;

    while (should_run.load())
    {
        use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);

        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = pro_nema;
            rads_acks_msg.cmd = 12;
            rads_acks_msg.cmd_opt = map_id;
            rads_acks_msg.ext1 = path_id;
            rads_acks_msg.ext2 = 1;
            rads_acks_msg.ack = 2;
            rads_acks_msg.map_id = map_id;
            rads_acks_msg.path_id = path_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建路线,在地图号：" << map_id << " ,创建了线路号：" << path_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建路线,在地图号：" << map_id << " ,创建了线路号：" << path_id << ",一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 接受网络指令
void pradscmdCallback(const rads::rads_cmd::ConstPtr &msg)
{
    sradsack_fast sradsack_fast_data;
    sradsack_fast_data.pro_nema = msg->pro_nema;
    sradsack_fast_data.cmd = msg->pro_cmd;
    sradsack_fast_data.cmd_opt = msg->pro_cmd_opt;
    sradsack_fast_data.ext1 = msg->pro_ext1;
    sradsack_fast_data.ext2 = msg->pro_ext2;

    ROS_INFO("Received pradscmd message:");
    ROS_INFO_STREAM("pro_nema: " << msg->pro_nema);
    ROS_INFO_STREAM("pro_head: " << msg->pro_head);
    ROS_INFO_STREAM("pro_vehsn: " << msg->pro_vehsn);
    ROS_INFO_STREAM("pro_cmd: " << msg->pro_cmd);
    ROS_INFO_STREAM("pro_cmd_opt: " << msg->pro_cmd_opt);
    ROS_INFO_STREAM("pro_ext1: " << msg->pro_ext1);
    ROS_INFO_STREAM("pro_ext2: " << msg->pro_ext2);
    for (int i = 0; i < msg->data.size(); ++i)
    {
        ROS_INFO_STREAM("data[" << i << "]: " << msg->data[i]);
    }
    // 任意点导航
    if (msg->pro_cmd == 30)
    {
        std::cout << "***********************" << "任意点导航开始" << "*****************************" << std::endl;
        // 地图    msg->pro_cmd_opt;
        // 点数    msg->pro_ext1;
        // msg->pro_ext2  0 ：预览  1：执行   2：执行带返航
        rads::rads_acks acksmsg;
        rads::rads_trackext trajectory_ext;
        acksmsg.header.stamp = ros::Time::now();
        acksmsg.header.frame_id = "sradsack_map";
        acksmsg.pro_nema = msg->pro_nema;
        acksmsg.cmd = msg->pro_cmd;
        acksmsg.cmd_opt = msg->pro_cmd_opt;
        acksmsg.ext1 = msg->pro_ext1;
        acksmsg.ext2 = msg->pro_ext2;
        acksmsg.ack = 2;
        acksmsg.map_id = msg->pro_cmd_opt;
        double x1, y1, x2, y2;
        std::vector<std::vector<int16_t>> all_paths;
        all_paths.clear();
        if (msg->pro_ext2 != 0) // 预览不清除表
        {
            // 清空表
        }
        // 获取所有基本点
        auto pathPoints = fetchPointallDataFromDB(msg->pro_cmd_opt);

        if (msg->pro_ext1 > 0 && msg->data.size() >= msg->pro_ext1 * 2) // 执行
        {
            x1 = lidar_rtk.x;
            y1 = lidar_rtk.y;
            x2 = msg->data[0] * 0.01;
            y2 = msg->data[1] * 0.01;
            all_paths.push_back(getPath_anypoint(x1, y1, x2, y2, pathPoints));
            for (int i = 0; i < (msg->pro_ext1 - 1) * 2; i += 2)
            {
                x1 = msg->data[i] * 0.01;
                y1 = msg->data[i + 1] * 0.01;
                x2 = msg->data[i + 2] * 0.01;
                y2 = msg->data[i + 3] * 0.01;
                all_paths.push_back(getPath_anypoint(x1, y1, x2, y2, pathPoints));
            }
        }

        // 遍历 all_paths 并打印每个路径点
        int num = 1;
        for (size_t i = 0; i < all_paths.size(); ++i)
        {
            for (size_t j = 0; j < all_paths[i].size(); ++j)
            {
                auto it = std::find_if(pathPoints.begin(), pathPoints.end(), [&](const point_all &p)
                                       { return p.num == all_paths[i][j] + 1; });
                if (it != pathPoints.end())
                {
                    const point_all &pointall = *it;
                    trajectory_ext.point_count = num;
                    trajectory_ext.alltotal = all_paths[i].size();
                    trajectory_ext.x = pointall.x;
                    trajectory_ext.y = pointall.y;
                    trajectory_ext.z = pointall.z;
                    trajectory_ext.ld_heading = pointall.lidar_heading;
                    trajectory_ext.matching_rate = pointall.ndt_matching;
                    trajectory_ext.rtk_x = pointall.rtk_x;
                    trajectory_ext.rtk_y = pointall.rtk_y;
                    trajectory_ext.rtk_z = pointall.rtk_z;
                    trajectory_ext.longitude = pointall.lng;
                    trajectory_ext.latitude = pointall.lat;
                    trajectory_ext.heading = pointall.rtk_heading;
                    trajectory_ext.posi_flag = pointall.position_flag;
                    trajectory_ext.head_flag = pointall.orientation_flag;
                    acksmsg.trajectory_ext.push_back(trajectory_ext);
                    num++;
                }
            }
        }
        sradsack_pub_.publish(acksmsg);

        stopRadarThread.store(true);
        sleep(2);
        // 获取该路径上的所有点位
        auto point_data_points = fetchPointdataDataFromDB();
        // 启动自动驾驶线程
        stopRadarThread.store(false);
        int loop_count = 1; // 循环次数（可改为参数或配置）
        std::thread radarThread(radarThreadFunction, std::ref(point_data_points) , loop_count);
        radarThread.detach();
    }

    // 创建基本线路（马踏飞燕）
    if (msg->pro_cmd == 7)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createline_all.joinable())
        {
            std::cout << "***********************" << "创建基本线路,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            // 检查地图是否需要切换
            if (getmap_id != msg->pro_cmd_opt)
            {
                getmap_id = msg->pro_cmd_opt;
                map_ck(msg->pro_cmd_opt);
                sleep(3);
            }
            should_run_all.store(true);
            thread_instance_createline_all = std::thread(std::bind(thread_Create_line_all, msg->pro_cmd_opt));
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }

        if (msg->pro_ext2 == 0 && thread_instance_createline_all.joinable())
        {
            should_run_all.store(false);
            if (thread_instance_createline_all.joinable())
            {
                thread_instance_createline_all.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建基本线路,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 终止任务
    if (msg->pro_cmd == 20)
    {
        std::cout << "***********************" << "终止任务" << "*****************************" << std::endl;
        stopRadarThread.store(true); // 发送停止信号
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.ack = 1;
        publishacks(sradsack_pub_, sradsack_fast_data);
    }

    // 调度路线
    if (msg->pro_cmd == 5)
    {
        std::cout << "**********去往" << msg->pro_cmd_opt << "号地图," << msg->pro_ext1 << "号线的" << msg->pro_ext2 << "号点位***************" << std::endl;
        // 检查地图是否需要切换
        if (getmap_id != msg->pro_cmd_opt)
        {
            getmap_id = msg->pro_cmd_opt;
            map_ck(msg->pro_cmd_opt);
            sleep(3);
        }
        // 获取该路径上的所有点位
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        sradsack_fast_data.ack = 1;
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.path_id = msg->pro_ext1;
        publishacks(sradsack_pub_, sradsack_fast_data);
        stopRadarThread.store(true);
        sleep(2);
        // 启动自动驾驶线程
        stop_car = 1;
        stopRadarThread.store(false);
        int loop_count = 1; // 循环次数（可改为参数或配置）
        std::thread radarThread(radarThreadFunction, pathPoints,loop_count);
        radarThread.detach();
    }
     // 调度路线--循环
    if (msg->pro_cmd == 6)
    {
        std::cout << "**********去往" << msg->pro_cmd_opt << "号地图," << msg->pro_ext1 << "号线，循环次数：" << msg->pro_ext2 << "***************" << std::endl;
        // 检查地图是否需要切换
        if (getmap_id != msg->pro_cmd_opt)
        {
            getmap_id = msg->pro_cmd_opt;
            map_ck(msg->pro_cmd_opt);
            sleep(3);
        }
        // 获取该路径上的所有点位
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        sradsack_fast_data.ack = 1;
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.path_id = msg->pro_ext1;
        publishacks(sradsack_pub_, sradsack_fast_data);
        stopRadarThread.store(true);
        sleep(2);
        // 启动自动驾驶线程
        stop_car = 1;
        stopRadarThread.store(false);
        int loop_count = msg->pro_ext2; // 循环次数（可改为参数或配置）
        std::thread radarThread(radarThreadFunction, pathPoints,loop_count);
        radarThread.detach();
    }
    // 临时停车
    if (msg->pro_cmd == 8)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "临时停车" << "*****************************" << std::endl;
            stop_car = 2;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "解除临时停车" << "*****************************" << std::endl;
            stop_car = 1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 紧急制动停车
    if (msg->pro_cmd == 9)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "紧急制动停车" << "*****************************" << std::endl;
            stop_car = 3;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "解除紧急制动停车" << "*****************************" << std::endl;
            stop_car = 1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 创建线路
    if (msg->pro_cmd == 12)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createline.joinable())
        {

            std::cout << "***********************" << "创建线路,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
            should_run.store(true);
            thread_instance_createline = std::thread(std::bind(thread_Create_line, msg->pro_cmd_opt, msg->pro_ext1, msg->pro_nema));
        }
        if (msg->pro_ext2 == 0 && thread_instance_createline.joinable())
        {
            should_run.store(false);
            if (thread_instance_createline.joinable())
            {
                thread_instance_createline.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建线路,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 开始记忆线路
    if (msg->pro_cmd == 2)
    {
        if (msg->pro_cmd_opt == 0 && !thread_instance_createline_goback.joinable()) // 开始记忆
        {

            std::cout << "***********************" << "启动记忆" << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./starttu.sh &");
            int result = system(command);
            sleep(3);
            should_run_goback.store(true);
            thread_instance_createline_goback = std::thread(std::bind(thread_Create_line_goback));
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_cmd_opt == 1) // 清除记忆
        {

            std::cout << "***********************" << "清除记忆开始" << "*****************************" << std::endl;
            // 关闭返航线程
            should_run_goback_play.store(false);
            if (thread_instance_createline_goback_play.joinable())
            {
                thread_instance_createline_goback_play.join();
                std::cout << "***********************" << "关闭返航线程" << "*****************************" << std::endl;
            }
            // 关闭记忆
            should_run_goback.store(false);
            if (thread_instance_createline_goback.joinable())
            {
                thread_instance_createline_goback.join(); // 等待线程结束
                std::cout << "***********************" << "关闭记忆" << "*****************************" << std::endl;
            }
            // 清空数据库
            delete_go_back();
            std::cout << "***********************" << "清空数据库" << "*****************************" << std::endl;

            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }

    // 开始返航
    if (msg->pro_cmd == 3)
    {
        if (msg->pro_cmd_opt == 0) //   开始返航
        {
            std::cout << "***********************" << "开始返航" << "*****************************" << std::endl;
            // 首先关闭记忆
            should_run_goback.store(false);
            if (thread_instance_createline_goback.joinable())
            {
                thread_instance_createline_goback.join(); // 等待线程结束
                std::cout << "***********************" << "关闭记忆" << "*****************************" << std::endl;
            }
            // 关闭自动驾驶
            stopRadarThread.store(true);
            pubdrive(0, 2, 18000, 0);
            sleep(1);
            // 创建返航线程
            if (!thread_instance_createline_goback_play.joinable())
            {
                // 首次启动返航线程

                stop_car = 1;
                current_target_index = 0;
                goback_forward = true;  // 初始方向：正向
                need_turnaround = true; // true  需要掉头  false 不需要掉头
                should_run_goback_play.store(true);
                thread_instance_createline_goback_play = std::thread(std::bind(thread_Create_line_goback_play));
                std::cout << "***********************" << "创建返航线程" << "*****************************" << std::endl;
            }
            else
            {
                if (should_run_goback_play.load())
                {
                    goback_forward = !goback_forward; // 反向行驶
                    stop_car = 1;
                    need_turnaround = true; // true  需要掉头  false 不需要掉头
                    std::cout << "切换方向为: " << (goback_forward ? "正向" : "反向")
                              << ", 当前点: " << current_target_index.load() << std::endl;
                    std::cout << "***********************" << "返航掉头" << "*****************************" << std::endl;
                }
                else
                {
                    std::cout << "***********************" << "返航线程未启动" << "*****************************" << std::endl;
                }
            }
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }

        if (msg->pro_cmd_opt == 1) //   暂停||继续
        {
            stop_car = (stop_car == 1) ? 2 : 1;
            std::cout << "***********************" << "暂停||继续：" << stop_car << "************************" << std::endl;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 设置开机点
    if (msg->pro_cmd == 16)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "上传开机点，地图编号:" << msg->pro_cmd_opt << "开机点编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            // 上报
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = msg->pro_nema;
            rads_acks_msg.cmd = msg->pro_cmd;
            rads_acks_msg.cmd_opt = msg->pro_cmd_opt;
            rads_acks_msg.ext1 = msg->pro_ext1;
            rads_acks_msg.ext2 = msg->pro_ext2;
            rads_acks_msg.ack = 1;
            rads_acks_msg.map_id = msg->pro_cmd_opt;
            nav_msgs::Path trajectory;
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = lidar_rtk.x;
            pose.pose.position.y = lidar_rtk.y;
            pose.pose.position.z = lidar_rtk.z;
            pose.pose.orientation.x = lidar_rtk.ox;
            pose.pose.orientation.y = lidar_rtk.oy;
            pose.pose.orientation.z = lidar_rtk.oz;
            pose.pose.orientation.w = lidar_rtk.ow;
            rads_acks_msg.trajectory.poses.push_back(pose);
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = msg->pro_ext1; // 开机点编号
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.x;
            trajectory_ext.rtk_y = lidar_rtk.y;
            trajectory_ext.rtk_z = lidar_rtk.z;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "删除开机点，地图编号:" << msg->pro_cmd_opt << "开机点编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 创建地图
    if (msg->pro_cmd == 10)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "开始建图,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./starttu.sh %d &", (int)msg->pro_cmd_opt);
            int result = system(command);
            sleep(10);
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "结束建图,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./endtu.sh %d &", (int)msg->pro_cmd_opt);
            int result = system(command);
            sleep(25);
            std::string map_path = "/home/argo/catkin_ws/pcd/" + std::to_string(msg->pro_cmd_opt) + ".pcd";
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.map_path = map_path;
            publishacks(sradsack_pub_, sradsack_fast_data);
            getmap_id = msg->pro_cmd_opt;
            map_ck(msg->pro_cmd_opt);
        }
    }
     // 上装功能，摄像头功能为41，抓拍:摄像头机位3,本次抓拍识别码32
    if (msg->pro_cmd == 4 && msg->pro_cmd_opt==1 )
    {

        std::cout << "***********************" << "抓拍:摄像头机位:" << msg->pro_ext1 << "本次抓拍识别码：" << msg->pro_ext2 << "*****************************" << std::endl;
        int ck = Capture(msg->pro_ext2);
        if(ck == 0){
        sradsack_fast_data.ack = 1;
        std::string img_path = "/home/argo/catkin_ws/src/rads-master/img/" + std::to_string(msg->pro_ext2) + ".jpeg";
        sradsack_fast_data.map_path = img_path;
        publishacks(sradsack_pub_, sradsack_fast_data);
        }else{
        sradsack_fast_data.ack = 2;
        publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
}
// 接受融合后的雷达+RTK数据
void laserCallback(const rads::rads_trackext::ConstPtr &msg)
{
    // 获取雷达数据
    lidar_rtk.x = isnan(msg->x) ? 0.0 : msg->x;
    lidar_rtk.y = isnan(msg->y) ? 0.0 : msg->y;
    lidar_rtk.z = isnan(msg->z) ? 0.0 : msg->z;
    lidar_rtk.ox = isnan(msg->ox) ? 0.0 : msg->ox;
    lidar_rtk.oy = isnan(msg->oy) ? 0.0 : msg->oy;
    lidar_rtk.oz = isnan(msg->oz) ? 0.0 : msg->oz;
    lidar_rtk.ow = isnan(msg->ow) ? 0.0 : msg->ow;
    lidar_rtk.matching_rate = isnan(msg->matching_rate) ? 0.0 : msg->matching_rate;
    // 处理四元数数据
    lidar_rtk.ld_heading = isnan(quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow)) ? 0.0 : quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow);
    //std::cout << "lidar_rtk.ld_heading:" << lidar_rtk.ld_heading <<std::endl;
    // 获取RTK数据
    double x1 = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    double y1 = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    double yaw = isnan(msg->heading) ? 0.0 : msg->heading;
    double baseline = RTK_BASELINE;
    double xMid, yMid;
    calculateMidPoint(x1, y1, yaw, baseline, xMid, yMid);
    lidar_rtk.rtk_x = isnan(yMid) ? 0.0 : yMid;
    lidar_rtk.rtk_y = isnan(xMid) ? 0.0 : xMid;
    lidar_rtk.rtk_z = isnan(msg->rtk_z) ? 0.0 : msg->rtk_z;
    lidar_rtk.heading = isnan(msg->heading) ? 0.0 : msg->heading;
    lidar_rtk.longitude = isnan(msg->longitude) ? 0.0 : msg->longitude;
    lidar_rtk.latitude = isnan(msg->latitude) ? 0.0 : msg->latitude;
    lidar_rtk.posi_flag = isnan(msg->posi_flag) ? 0.0 : msg->posi_flag;
    lidar_rtk.head_flag = isnan(msg->head_flag) ? 0.0 : msg->head_flag;
    lidar_rtk.groud_speed = isnan(msg->groud_speed) ? 0.0 : msg->groud_speed;
    lidar_rtk.base_length = isnan(msg->flag) ? 0.0 : msg->flag;
}
// 避障（停）
void points_rawCallback(const sensor_msgs::PointCloud2ConstPtr &msg_pc)
{

// MAX_X: 0.4  
// MIN_X: -0.4
// MAX_Y: 1.2
// MIN_Y: 0.1
// MIN_Z: -0.3
    pcl::PointCloud<pcl::PointXYZ> pc;
    pcl::fromROSMsg(*msg_pc, pc);
    int Stop_3Dpoint = 0;
    int Stop_3Dpoint_qian=0;
    int Stop_3Dpoint_l=0;
    int Stop_3Dpoint_r=0;
    for (const pcl::PointXYZ &p : pc.points)
    {
        if (p.x <= MAX_X && p.x >= MIN_X && p.z >= MIN_Z && p.z <= 5.0 && p.y >= MIN_Y && p.y <= MAX_Y) // 停障区(p.x)/激光雷达左右宽度,(p.y);//激光雷达前后(p.z);//激光雷达高度差
        {
            Stop_3Dpoint++;
        }
        if (p.x <= 0.4 && p.x >= -0.4 && p.z >= -0.3 && p.z <= 5.0 && p.y >= 1.2 && p.y <= 2.0) // 停障区(p.x)/激光雷达左右宽度,(p.y);//激光雷达前后(p.z);//激光雷达高度差
        {
            Stop_3Dpoint_qian++;   //绕障碍区
        }
        if (p.x <= -0.4 && p.x >= -1.0 && p.z >= -0.3 && p.z <= 5.0 && p.y >= -0.9 && p.y <= 2.0) // 停障区(p.x)/激光雷达左右宽度,(p.y);//激光雷达前后(p.z);//激光雷达高度差
        {
            Stop_3Dpoint_l++;     //左侧
        }
        if (p.x <= 1.0 && p.x >= 0.4 && p.z >= -0.3 && p.z <= 5.0 && p.y >= -0.9 && p.y <= 2.0) // 停障区(p.x)/激光雷达左右宽度,(p.y);//激光雷达前后(p.z);//激光雷达高度差
        {
            Stop_3Dpoint_r++;    //右侧
        }
    }
    if (RADAR_TEST == 1)
    {
        std::cout << "=====================================" << std::endl;
        std::cout << "前方3D点数:" << Stop_3Dpoint << std::endl;
        std::cout << "最前3D点数:" << Stop_3Dpoint_qian << std::endl;
        std::cout << "左前3D点数:" << Stop_3Dpoint_l << std::endl;
        std::cout << "右前3D点数:" << Stop_3Dpoint_r << std::endl;
    }
    ros::param::get("RADAR_F", RADAR_F);
    if (Stop_3Dpoint > STOP_3D_PONIT && RADAR_F == 1)
    {
        Stop_lidar = 1; // 遇到障碍停车
    }
    else
    {
        Stop_lidar = 0; // 没有障碍正常跑
    }
    if (Stop_3Dpoint_qian > STOP_3D_PONIT)
    {
        Stop_lidar_qian = 1;   
    }else{
        Stop_lidar_qian = 0;    
    }

    if (Stop_3Dpoint_r > STOP_3D_PONIT)
    {
        Stop_lidar_r = 1;   
    }else{
        Stop_lidar_r = 0;    
    }

    if (Stop_3Dpoint_l > STOP_3D_PONIT)
    {
        Stop_lidar_l = 1;   
    }else{
        Stop_lidar_l = 0;    
    }
        //  std::cout << "=====================================" << std::endl;
        // std::cout << "前方3D点数:" << Stop_lidar << std::endl;
        // std::cout << "最前3D点数:" << Stop_lidar_qian << std::endl;
        // std::cout << "左前3D点数:" << Stop_lidar_l << std::endl;
        // std::cout << "右前3D点数:" << Stop_lidar_r << std::endl;

}
// 下位机来的数据
void pradsscmdCallback(const rads::rads_scmd::ConstPtr &msg)
{
    int model = msg->model; // （1:遥控，2:跟随和推，3:返航，4:无人驾驶）
    if (model != 4)         // 模式不等于4
    {
        stopRadarThread.store(true);         // 关闭自动驾驶
        should_run_goback_play.store(false); // 关闭返航
    }
}
//双光谱摄像头报警
void pradscameraCallback(const rads::rads_camera::ConstPtr &msg)
{   
    ros::param::get("KTV_SN", KTV_SN);
    int device_id = std::stoi(KTV_SN);
    
    // 获取当前位置
    double current_latitude = lidar_rtk.latitude;
    double current_longitude = lidar_rtk.longitude;
    
    // 获取当前时间
    ros::Time current_time = ros::Time::now();
    
    // 检查是否需要发布报警
    bool should_publish = false;
    
    // 首次报警或满足距离/时间条件
    if (is_first_alarm) {
        should_publish = true;
        is_first_alarm = false;
    } else {
        // 计算距离（米）
        double distance = calculateDistance(last_latitude, last_longitude, current_latitude, current_longitude);
        
        // 计算时间差（秒）
        ros::Duration time_diff = current_time - last_alarm_time;
        
        // 判断是否满足条件：距离大于5米或时间超过3分钟
        should_publish = (distance > 5.0) || (time_diff.toSec() > 180.0);
    }
    
    if (should_publish) {
        // 更新最后报警位置和时间
        last_latitude = current_latitude;
        last_longitude = current_longitude;
        last_alarm_time = current_time;
        
        // 执行拍照
        int ck = Capture(device_id);
        
        if (ck == 0) {
            // 复制消息内容
            rads::rads_camera msginfo;
            msginfo.device_id = std::to_string(device_id);
            msginfo.devIp = msg->devIp;
            msginfo.Alarmtype = msg->Alarmtype;
            msginfo.preset = msg->preset;
            msginfo.preAlarmSum = msg->preAlarmSum;
            msginfo.alarmSum = msg->alarmSum;
            msginfo.mtAlarmInfo = msg->mtAlarmInfo;
            msginfo.file = "/home/argo/catkin_ws/src/rads-master/img/" + std::to_string(device_id) + ".jpeg";
            msginfo.thermalfile = "/home/argo/catkin_ws/src/rads-master/reimg/" + std::to_string(device_id) + ".jpeg";
            
            // 发布报警消息
            pradscamerapub.publish(msginfo);
            
            // 使用ROS时间格式化函数替代toString()
            //char time_str[100];
            //sprintf(time_str, "%u.%09u", current_time.sec, current_time.nsec);
            //ROS_INFO("Alarm published at position: lat=%.6f, lon=%.6f, time=%s", 
            //         current_latitude, current_longitude, time_str);
            
            ck = -1;
        }
    } else {
        // 使用ROS时间格式化函数替代toString()
        //char time_str[100];
        //sprintf(time_str, "%u.%09u", current_time.sec, current_time.nsec);
       // ROS_INFO("Alarm suppressed: distance=%.2f m, time_diff=%.1f s, current_time=%s", 
        //         calculateDistance(last_latitude, last_longitude, current_latitude, current_longitude),
        //         (current_time - last_alarm_time).toSec(), time_str);
    }
}


// 接受goback定位
void odometryCallgoback(const nav_msgs::Odometry::ConstPtr &msg)
{

    lidar_gobackmsg.x = isnan(msg->pose.pose.position.x) ? 0.0 : msg->pose.pose.position.x;
    lidar_gobackmsg.y = isnan(msg->pose.pose.position.y) ? 0.0 : msg->pose.pose.position.y;
    lidar_gobackmsg.z = isnan(msg->pose.pose.position.z) ? 0.0 : msg->pose.pose.position.z;
    lidar_gobackmsg.ld_heading = isnan(quaternionToEulerAngle(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w)) ? 0.0 : quaternionToEulerAngle(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
}



// 接受imu  sensor_msgs::Imu
void callbackimu(const sensor_msgs::Imu &msg)
{
    imu_msg.x = msg.orientation.x;
    imu_msg.y = msg.orientation.y;
    imu_msg.z = msg.orientation.z;
}
int main(int argc, char **argv)
{

    ros::init(argc, argv, NODE_BUSINESS);
    ros::NodeHandle nh_;
    ros::param::get("AUTO_DISRANCE", AUTO_DISRANCE);
    ros::param::get("HEADING_THRESHOLD", HEADING_THRESHOLD);
    ros::param::get("DRIVE_DISRANCE", DRIVE_DISRANCE);
    ros::param::get("DRIVE_DISRANCE_LAST", DRIVE_DISRANCE_LAST);
    ros::param::get("MAXSPEED", MAXSPEED);
    ros::param::get("MINSPEED", MINSPEED);
    ros::param::get("MAX_X", MAX_X);
    ros::param::get("MIN_X", MIN_X);
    ros::param::get("MAX_Y", MAX_Y);
    ros::param::get("MIN_Y", MIN_Y);
    ros::param::get("MIN_Z", MIN_Z);
    ros::param::get("STOP_3D_PONIT", STOP_3D_PONIT);
    ros::param::get("RADAR_TEST", RADAR_TEST);
    ros::param::get("HEADING_ERROR", HEADING_ERROR);
    ros::param::get("RTK_BASELINE", RTK_BASELINE);
    ros::param::get("PID_STATUS", PID_STATUS);
    ros::param::get("PID_STATUS", CONFIG_KP);
    ros::param::get("PID_STATUS", CONFIG_KI);
    ros::param::get("PID_STATUS", CONFIG_KD);
    ros::param::get("PID_STATUS", CONFIG_MIN_OUTPUT);
    ros::param::get("PID_STATUS", CONFIG_MAX_OUTPUT);
    getmap_id = getmapid();
    map_ck(getmap_id);
    //Demo_Capture();
    // 订阅网络发来指令
    static ros::Subscriber sub_ = nh_.subscribe("pradscmd", 1000, pradscmdCallback);
    // 订阅融合后的雷达+RTK数据
    static ros::Subscriber ndt_pose = nh_.subscribe("pradsfusion", 1, laserCallback); // ndt_pose
    // 订阅points_raw 用于激光雷达停障
    static ros::Subscriber sub_points_raw = nh_.subscribe<sensor_msgs::PointCloud2>("/points_raw", 100, points_rawCallback);
    // 订阅下位机发来的指令 pradsscmd
    static ros::Subscriber sub_pradsscmd = nh_.subscribe("pradsscmd", 1, pradsscmdCallback);
    // 订阅返航定位
    static ros::Subscriber sub_pradsscmd_goback_weizhi = nh_.subscribe("/odometry/imu", 1, odometryCallgoback);
     // 订阅双光普摄像头报警信息
    static ros::Subscriber sub_pradscamera = nh_.subscribe("pradscamera", 1,pradscameraCallback);
    // sensor_msgs::Imu
    static ros::Subscriber imusub = nh_.subscribe("imu603", 1, callbackimu);
    // 发布响应网络信息（主要用于响应网络指令，上传地图，上传路线点等）
    sradsack_pub_ = nh_.advertise<rads::rads_acks>("sradsack", 1000);
    // 发布控制车辆的信息
    sradsdrive = nh_.advertise<rads::rads_scmdret>("sradsdriveackermann", 10);
    // 发布地图重定位
    initialposepub = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
    // 发布双光普摄像头报警信息给服务器
    pradscamerapub = nh_.advertise<rads::rads_camera>("pradscamerainfo", 10);

    ros::spin();
    return 0;
}
